#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import simulate_platform
import servonode_core
import time
from config import *

def car_should_stop():
    print("Car stopped")
    return (abs(p.speed) < MIN_DIFF)

def car_should_move(speed):
    print(f"Car moving {p.speed:6.2f} should be {speed:6.2f}")
    return abs(p.speed - speed) < MIN_DIFF
def car_should_turn(angle):
    print(f"Car turning {p.steering:6.2f} should be {angle:6.2f}")
    #print(f"turning {p.steering:6.f}")
    return abs(p.steering - angle) < MIN_DIFF
def car_should_wait_at_zebra():
    print(f"Car state {p.state}")
    return p.state == "wait_at_zebra"

def wait_1_second():
    time.sleep(1)

#         + 交通信号灯，traffic_valid 0 没有检测到交通信号灯，1 红灯，2绿灯，3黄灯
#         |   + 雷达检测前方20度视野内20cm距离内 0 无障碍物，1有障碍物
#         |   |   + 斑马线检测，0 前方20cm没有斑马线，1 前方20cm有斑马线
#         |   |   |     + 雷达检测左右挡板，0 直行， <0 左转，>0 右转
#         |   |   |     |     + 车道线检测信息，None 无检测结果，0 直行， <0 左转，>0 右转
#         |   |   |     |     |     + 限速和解除限速标识, None 无检测结果，0 解除限速，1 限速
test_pattern_1 = (\
        ((0,  0,  0,    0, None, None), (car_should_stop, None)), \
        ((1,  0,  0,    0, None, None), (car_should_stop, None)), \
        (("time.sleep(1)",           ), (car_should_stop, None)), \
        ((0,  0,  0,    0, None, None), (car_should_stop, None)), \
        ((wait_1_second,             ), (car_should_stop, None)), \
        ((2,  0,  0,    0, None, None), (car_should_move, HIGH_SPEED)), \
        ((0,  0,  0,    0, -5.0, None), (car_should_turn, -5.0)), \
        ((0,  0,  0,  3.0, None, None), (car_should_turn,  3.0)), \
        ((wait_1_second,             ), (car_should_move, HIGH_SPEED)), \
        ((0,  1,  0,  3.0, None, None), (car_should_stop, None)), \
        ((0,  0,  0,  0.0, None, None), (car_should_move, HIGH_SPEED)), \
        ((0,  0,  1,  0.0, None, None), (car_should_wait_at_zebra, None)), \
        ((wait_1_second,             ), (car_should_stop, None)), \
        (("time.sleep(0.9)",           ), (car_should_stop, None)), \
        (("time.sleep(5.0)",           ), (car_should_move, HIGH_SPEED)), \
        ((0,  0,  1,  0.0, None, None), (car_should_wait_at_zebra, None)), \
        ((wait_1_second,             ), (car_should_stop, None)), \
        (("time.sleep(1.5)",           ), (car_should_move, HIGH_SPEED)), \
        ((0,  1,  0,  0.0, None, None), (car_should_stop, None)), \
        ((0,  0,  0,  0.0, None, None), (car_should_move, HIGH_SPEED)), \
        )

test_pattern_2 = (\
        ((0,  0,  0,    0, None, None), (car_should_stop, None)), \
        ((1,  0,  0,    0, None, None), (car_should_stop, None)), \
        (("time.sleep(1)",           ), (car_should_stop, None)), \
        ((2,  0,  0,    0, None, None), (car_should_move, HIGH_SPEED)), \
        ((0,  0,  0,    0, -5.0, None), (car_should_turn, -5.0)), \
        ((0,  0,  0,  3.0, None, None), (car_should_turn,  3.0)), \
        ((wait_1_second,             ), (car_should_move, HIGH_SPEED)), \
        ((0,  0,  0,    0, None,    1), (car_should_move, LOW_SPEED)), \
        ((0,  0,  0,    0, None, None), (car_should_move, LOW_SPEED)), \
        ((0,  0,  0,    0, None,    0), (car_should_move, HIGH_SPEED)), \
        )

all_pattern = (test_pattern_1, test_pattern_2)
p = None
for pattern in all_pattern:
    testbench = simulate_platform.SimulatedTestbench(pattern)
    p = servonode_core.Process(testbench)
    p.process()

